#include "PTZ.h"

static uint32_t Servo_AngleToPulse_On(float angle)
{
    if (angle < 9) angle = 9;
    if (angle > 171) angle = 171;
    return 250 + (uint32_t)((angle / 180) * (float)(1250 - 250));
}

static uint32_t Servo_AngleToPulse_Under(float angle)
{
    if (angle < 0) angle = 0;
    if (angle > 270) angle = 270;
    return 250 + (uint32_t)((angle / 270) * (float)(1250 - 250));
}

// 初始化函数：传入 TIM1 句柄
void RobotArm_Init(void)
{

    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);

    // 初始化到中位
    Servo_SetPitch(90);
    Servo_SetYaw(180);
}

void Servo_SetPitch(float angle)
{
    uint32_t pulse = Servo_AngleToPulse_On(angle);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, pulse);
}

// 水平舵机：0°~270°
void Servo_SetYaw(float angle)
{
    uint32_t pulse = Servo_AngleToPulse_Under(angle);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, pulse);

}



